/************************************************************************************
 * @brief 一维卡尔曼估算示例
 *
 *    #include <cstdio>
 *    #include <cstdlib>
 *    #include <cmath>
 *    #include <cstring>
 *    #include <random>
 *    
 *    #include <hude/utils/kalman.hpp>
 *    
 *    //-----------------------------------------------------
 *    // 平滑度，取值大则收敛快, 取值小则估算平滑, 与估算误差正比.
 *    //   0.01~0.0001是最常用的范围
 *    static const double BEKK  = 0.005f;
 *    // 观测的标准差
 *    static const double ERROR = 1.2f;
 *    // 对高斯随机数放大系数
 *    static const size_t MULTI  = 100;
 *    // X轴坐标空间，生成高斯随机数的总数
 *    static const size_t XASIX = 500U;
 *    // Y轴坐标空间, Y为横轴
 *    static const size_t YASIX = 100U;
 *    
 *    
 *    //-----------------------------------------------------
 *    int main()
 *    {
 *        std::random_device rd{};
 *        std::mt19937 gen{ rd() };
 *    
 *        // 声明0均值标准差为ERROR的正态随机数生成器
 *        std::normal_distribution<> dist{ 0, ERROR };
 *    
 *        // 声明一个卡尔曼滤波器，初值0,初始估算方差MULTI，估算平滑系数BEKK
 *        //  初始估算方差通常可以设为最大估算值，因为估算方差自动收敛速度很快
 *        hude::utils::kalman_t<> kalman( 0, MULTI, BEKK );
 *    
 *        char buffer[YASIX + 1];
 *        buffer[ YASIX ] = '\0';
 *    
 *        for( size_t i = 1; i <= XASIX; ++i )  // 打印X轴，X为纵轴
 *        {
 *            double v; // 观测值
 *    
 *            {
 *                auto o = dist( gen ); // 生成原始的正态随机数
 *                auto r = cos( i / double( XASIX ) * 4 * 360U * 3.1415926 / 180 ); // 余弦值叠加值
 *    
 *                // 将正态随机的范围缩放到接近于0～1之间, 也就是做4个标准差之间的变换
 *                v = ( o + ERROR * 4.0f ) / ( ERROR * 4.0f * 2.0f );
 *    
 *                v = r / 4 + v; // 为随机数叠加余弦变化
 *                v = MULTI * v; // 将随机数按倍数放大
 *            }
 *    
 *            auto e = kalman( v, MULTI * ERROR ); // 估算值, MULTI * ERROR为近似的观测误差
 *    
 *            // 打印结果
 *            {
 *                size_t index_v = v / ( MULTI * 1 ) * YASIX;  // 观测值图形坐标
 *                size_t index_e = e / ( MULTI * 1 ) * YASIX;  // 估算值图形坐标
 *    
 *                memset( buffer, '.', YASIX );  // 清理打印缓存
 *    
 *                if( index_v < YASIX ) buffer[size_t(index_v)] = '+';  // 绘制观测值坐标点
 *                if( index_e < YASIX ) buffer[size_t(index_e)] = '*';  // 绘制估算值坐标点
 *    
 *                // 输出当前观测值与估算值的信息
 *                printf( "%03u: |%s|  v(%05d) >>> e(%05d)\n", i, buffer, (int)v, (int)e );
 *            }
 *        }
 *    
 *        return 0;
 *    }
 */

#ifndef  __kalman_8u98y78jiuiuyt6r565e4__
#define  __kalman_8u98y78jiuiuyt6r565e4__

namespace hude
{
    namespace utils
    {
        /************************************************************************************
         * @brief 一维卡尔曼估算
         */
        template< typename _value >
        struct _kalman_t
        {
            typedef _value value_t;

            /************************************************************************************
             * @brief ...
             */
            value_t var_;
            value_t err_;

            /************************************************************************************
             * @brief ...
             */
            const value_t wgt_;

            /************************************************************************************
             * @brief 构造函数
             * @param v: 初始评估值
             * @param e: 初始评估误差
             */
            _kalman_t( value_t v, value_t e, value_t w = 0.001f ) :
                var_( v ), err_( e ), wgt_( w )
            {
            }

            /************************************************************************************
             * @brief 构造函数
             */
            _kalman_t() : _kalman_t( value_t( 1.0f ), value_t( 0.1f ), value_t( 0.01f ) ) {}

            /************************************************************************************
             * @brief 重置状态
             *  @param v: 评估值
             *  @param e: 评估误差
             */
            inline void set( value_t v, value_t e )
            {
                var_ = v;
                err_ = e;
            }

            /************************************************************************************
             * @brief 根据新的观测值做出评估
             * @param m: 观测值
             * @param e: 观测误差
             * @param m: 观测值
             * @param e: 观测误差
             * @return 返回估算值
             */
            inline static value_t _evaluate( value_t mv, value_t me, value_t wt, value_t& ev, value_t& ee )
            {
                // 计算kalman增益
                value_t kgain = ee / ( ee + me );
                // 重新评估误差范围
                ee = ( 1 - kgain ) * ee + me * wt;
                // 评估当前的最优值
                ev = ev + kgain * ( mv - ev );

                return ev;
            }

            /************************************************************************************
             * @brief 根据新的观测值做出评估
             * @param m: 观测值
             * @param e: 观测误差
             * @return 返回估算值
             */
            inline value_t evaluate( value_t mv, value_t me = 1 )
            {
                return _evaluate( mv, me, wgt_, var_, err_ );
            }

            /************************************************************************************
             * @brief 根据新的观测值做出评估
             * @param m: 观测值
             * @param e: 观测误差
             * @return 返回估算值
             */
            inline value_t operator()( value_t mv, value_t me = 1 )
            {
                return _evaluate( mv, me, wgt_, var_, err_ );
            }

        };

        /************************************************************************************
         * @brief 简化别名
         */
        template< typename _value = double >
        using kalman_t = _kalman_t< _value >;

    } //namespace utils
} //namespace hude

#endif //__kalman_8u98y78jiuiuyt6r565e4__
